#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "moveit_custom_demo");
  ros::NodeHandle node_handle; 
  ros::AsyncSpinner spinner(1);
  spinner.start();
  moveit::planning_interface::MoveGroupInterface group("home");

  // 设置机器人回到初始状态 "zero"
  group.setNamedTarget("zero");
  group.setPlanningTime(100.0);
  // 进行运动规划，计算机器人回到初始状态的运动轨迹
  moveit::planning_interface::MoveGroupInterface::Plan zero_plan;
  moveit::core::MoveItErrorCode zero_success = group.plan(zero_plan);

  ROS_INFO("Visualizing plan to zero position %s", zero_success ? "" : "FAILED");

  // 访问规划的轨迹
  const moveit_msgs::RobotTrajectory& zero_trajectory = zero_plan.trajectory_;

  // 打印轨迹点
  for (size_t i = 0; i < zero_trajectory.joint_trajectory.points.size(); ++i)
  {
    const trajectory_msgs::JointTrajectoryPoint& point = zero_trajectory.joint_trajectory.points[i];
    ROS_INFO("Zero Waypoint %zu:", i);
    for (size_t j = 0; j < point.positions.size(); ++j)
    {
      ROS_INFO("Joint %zu: %f", j, point.positions[j]);
    }
  }
  // 让机械臂回到初始状态 "zero"
  if (zero_success)
    group.execute(zero_plan);
   // 获取机器人当前状态
  robot_state::RobotStatePtr current_state = group.getCurrentState();
  // 获取机器人当前末端的位姿
  const Eigen::Isometry3d& end_effector_pose = current_state->getGlobalLinkTransform("link6");
  // 获取位姿的旋转部分（四元数）
  Eigen::Quaterniond quaternion(end_effector_pose.rotation());
  // 打印初始状态的xyzw值
  ROS_INFO("Initial Pose (xyzw): %.3f %.3f %.3f %.3f",
           quaternion.x(), quaternion.y(), quaternion.z(), quaternion.w());
  // 设置机器人终端的目标位置
  //geometry_msgs::Pose target_pose1;
  //target_pose1.orientation.w = -0.0422;
  //target_pose1.orientation.x= 0.351275;
  //target_pose1.orientation.y = -0.0152888;
  //target_pose1.orientation.z = 0.935194;

  //target_pose1.position.x = 0.237649;
  //target_pose1.position.y = 0;
  //target_pose1.position.z = 0.32222;

  geometry_msgs::Pose target_pose1;
  target_pose1.orientation.w = 0.22;
  target_pose1.orientation.x= 0.343;
  target_pose1.orientation.y = -0.526;
  target_pose1.orientation.z = 0.746;

  target_pose1.position.x = 0.005;
  target_pose1.position.y = 0.09;
  target_pose1.position.z = 0.284;





  group.setPoseTarget(target_pose1);
  group.setGoalTolerance(10);

  // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
  moveit::planning_interface::MoveGroupInterface::Plan my_plan;
  moveit::core::MoveItErrorCode success = group.plan(my_plan);

  ROS_INFO("Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
  //ROS_INFO_STREAM("Planning failed with error code: " << moveit::core::MoveItErrorCode(success).toString());
 // ROS_INFO_STREAM("Planning failed with error code: " << moveit::core::MoveItErrorCode(success).getVal() << " (" << moveit::core::MoveItErrorCode(success).asStr() << ")");


  // 访问规划的轨迹
  const moveit_msgs::RobotTrajectory& trajectory = my_plan.trajectory_;

  // 打印轨迹点
  for (size_t i = 0; i < trajectory.joint_trajectory.points.size(); ++i)
  {
    const trajectory_msgs::JointTrajectoryPoint& point = trajectory.joint_trajectory.points[i];
    ROS_INFO("Waypoint %zu:", i);
    for (size_t j = 0; j < point.positions.size(); ++j)
    {
      ROS_INFO("Joint %zu: %f", j, point.positions[j]);
    }
  }

  // 让机械臂按照规划的轨迹开始运动。
  if (success)
    group.execute(my_plan);

  ros::shutdown(); 
  return 0;
}

